#include <NL/nl.h>

#include <fstream>
#include <cstdlib>
#include <cstring>
#include <vector>
#include <set>
#include <cmath>
#include <cassert>
#include <string>
#include <iostream>
#include <algorithm>
#include <sstream>

#include "utility.h"
#include "mesh.h"


class LSCM {
public:

    LSCM(Mesh & m) : mesh_(&m) {
    }


    // Outline of the algorithm:

    // 1) Find an initial solution by projecting on a plane
    // 2) Lock two vertices of the mesh
    // 3) Copy the initial u,v coordinates to OpenNL
    // 3) Construct the LSCM equation with OpenNL
    // 4) Solve the equation with OpenNL
    // 5) Copy OpenNL solution to the u,v coordinates


    void apply() {
		int nb_vertices = mesh_->vertices.size() ; // MODIFIED
        project() ;
        nlNewContext() ;

		nlSolverParameteri(NL_SOLVER, NL_CG) ;
        nlSolverParameteri(NL_PRECONDITIONER, NL_PRECOND_JACOBI) ;


        nlSolverParameteri(NL_NB_VARIABLES, 2*nb_vertices) ;
        nlSolverParameteri(NL_LEAST_SQUARES, NL_TRUE) ;
        nlSolverParameteri(NL_MAX_ITERATIONS, 5*nb_vertices) ;
        nlSolverParameterd(NL_THRESHOLD, 1e-10) ;
        nlBegin(NL_SYSTEM) ;
        mesh_to_solver() ;
        nlBegin(NL_MATRIX) ;
        setup_lscm() ;
        nlEnd(NL_MATRIX) ;
        nlEnd(NL_SYSTEM) ;
        std::cout << "Solving ..." << std::endl ;
        nlSolve() ;
        solver_to_mesh() ;
        double time ;
        NLint iterations;
        nlGetDoublev(NL_ELAPSED_TIME, &time) ;
        nlGetIntergerv(NL_USED_ITERATIONS, &iterations); 
        std::cout << "Solver time: " << time << std::endl ;
        std::cout << "Used iterations: " << iterations << std::endl ;
        nlDeleteContext(nlGetCurrent()) ;
    }

protected:

    void setup_lscm() {
		std::cout<<"FACE SIZE "<<mesh_->v_faces.size()<<std::endl;
		for(unsigned int i=0; i<mesh_->v_faces.size() ; i++) {
            setup_lscm(mesh_->v_faces[i]) ;
        }
    }

    // Note: no-need to triangulate the facet,
    // we can do that "virtually", by creating triangles
    // radiating around vertex 0 of the facet.
    // (however, this may be invalid for concave facets)
    void setup_lscm(const Face  *  face) {  //FIXME HERE
		
		HalfEdge * iter = face->he;
		setup_conformal_map_relations( * mesh_->vertices[iter->from_v->idx] ,
			* mesh_->vertices[iter->next->from_v->idx], * mesh_->vertices[iter->next->next->from_v->idx]) ;

    }

    // Computes the coordinates of the vertices of a triangle
    // in a local 2D orthonormal basis of the triangle's plane.
    static void project_triangle(
        const Vector3& p0, 
        const Vector3& p1, 
        const Vector3& p2,
        Vector2& z0,
        Vector2& z1,
        Vector2& z2
    ) {
        Vector3 X = p1 - p0 ;
        X.normalize() ;
        Vector3 Z = X ^ (p2 - p0) ;
        Z.normalize() ;
        Vector3 Y = Z ^ X ;
        const Vector3& O = p0 ;
        
        float x0 = 0 ;
        float y0 = 0 ;
        float x1 = (p1 - O).length() ;
        float y1 = 0 ;
        float x2 = (p2 - O) * X ;
        float y2 = (p2 - O) * Y ;        
            
        z0 = Vector2(x0,y0) ;
        z1 = Vector2(x1,y1) ;
        z2 = Vector2(x2,y2) ;        
    }

    // LSCM equation, geometric form :
    // (Z1 - Z0)(U2 - U0) = (Z2 - Z0)(U1 - U0)
    // Where Uk = uk + i.vk is the complex number 
    //                       corresponding to (u,v) coords
    //       Zk = xk + i.yk is the complex number 
    //                       corresponding to local (x,y) coords
    // cool: no divide with this expression,
    //  makes it more numerically stable in
    //  the presence of degenerate triangles.
    
    void setup_conformal_map_relations(
         Vertex& v0,  Vertex& v1,  Vertex& v2
    ) {
        
        int id0 = v0.idx ;
        int id1 = v1.idx ;
        int id2 = v2.idx ;
            
        const Vector3& p0 = v0.point ; 
        const Vector3& p1 = v1.point ;
        const Vector3& p2 = v2.point ;
            
        Vector2 z0,z1,z2 ;
        project_triangle(p0,p1,p2,z0,z1,z2) ;
        Vector2 z01 = z1 - z0 ;
        Vector2 z02 = z2 - z0 ;
        double a = z01.x ;
        double b = z01.y ;
        double c = z02.x ;
        double d = z02.y ;
        assert(b == 0.0) ;

        // Note  : 2*id + 0 --> u
        //         2*id + 1 --> v
        int u0_id = 2*id0     ;
        int v0_id = 2*id0 + 1 ;
        int u1_id = 2*id1     ;
        int v1_id = 2*id1 + 1 ;
        int u2_id = 2*id2     ;
        int v2_id = 2*id2 + 1 ;
        
        // Note : b = 0

        // Real part
        nlBegin(NL_ROW) ;
        nlCoefficient(u0_id, -a+c)  ;
        nlCoefficient(v0_id,  b-d)  ;
        nlCoefficient(u1_id,   -c)  ;
        nlCoefficient(v1_id,    d)  ;
        nlCoefficient(u2_id,    a) ;
        nlEnd(NL_ROW) ;

        // Imaginary part
        nlBegin(NL_ROW) ;
        nlCoefficient(u0_id, -b+d) ;
        nlCoefficient(v0_id, -a+c) ;
        nlCoefficient(u1_id,   -d) ;
        nlCoefficient(v1_id,   -c) ;
        nlCoefficient(v2_id,    a) ;
        nlEnd(NL_ROW) ;
    }

    /**
     * copies u,v coordinates from OpenNL solver to the mesh.
     */
    void solver_to_mesh() {
		for(unsigned int i=0; i<mesh_->vertices.size(); i++) {
            Vertex * & it = mesh_->vertices[i]; 
            float u = nlGetVariable(2 * it->idx    ) ;
            float v = nlGetVariable(2 * it->idx + 1) ;
            it->tex_coord = Vector2(u,v) ;
        }
    }
        
    /**
     * copies u,v coordinates from the mesh to OpenNL solver.
     */
    void mesh_to_solver() {
		for(unsigned int i=0; i<mesh_->vertices.size(); i++) {
            Vertex * it = mesh_->vertices[i] ;
            float u = it->tex_coord.x ;
            float v = it->tex_coord.y ;
			//std::cout<<u<<' '<<v<<std::endl;
            nlSetVariable(2 * it->idx    , u) ;
            nlSetVariable(2 * it->idx + 1, v) ;
            if(it->locked) {
                nlLockVariable(2 * it->idx    ) ;
                nlLockVariable(2 * it->idx + 1) ;
            } 
        }
    }

    // Chooses an initial solution, and locks two vertices
    void project() {
        // Get bbox
        unsigned int i ;

        float xmin =  1e5 ;
        float ymin =  1e5 ;
        float zmin =  1e5 ;
        float xmax = -1e5 ;
        float ymax = -1e5 ;
        float zmax = -1e5 ;

        for(i=0; i<mesh_->vertices.size(); i++) {
            const Vertex *  v = mesh_->vertices[i] ;
            xmin = std::min(v->point.x, xmin) ;
            ymin = std::min(v->point.y, xmin) ;
            zmin = std::min(v->point.z, xmin) ;

            xmax = std::max(v->point.x, xmin) ;
            ymax = std::max(v->point.y, xmin) ;
            zmax = std::max(v->point.z, xmin) ;
        }

        float dx = xmax - xmin ;
        float dy = ymax - ymin ;
        float dz = zmax - zmin ;
        
		std::cout<<"DX "<<dx<<std::endl;
		std::cout<<"DY "<<dy<<std::endl;
		std::cout<<"DZ "<<dz<<std::endl;
        Vector3 V1,V2 ;

        // Find shortest bbox axis
        if(dx < dy && dx < dz) {
            if(dy > dz) {
                V1 = Vector3(0,1,0) ;
                V2 = Vector3(0,0,1) ;
            } else {
                V2 = Vector3(0,1,0) ;
                V1 = Vector3(0,0,1) ;
            }
        } else if(dy < dx && dy < dz) {
            if(dx > dz) {
                V1 = Vector3(1,0,0) ;
                V2 = Vector3(0,0,1) ;
            } else {
                V2 = Vector3(1,0,0) ;
                V1 = Vector3(0,0,1) ;
            }
        } else  {
            if(dx > dy) {
                V1 = Vector3(1,0,0) ;
                V2 = Vector3(0,1,0) ;
            } else {
                V2 = Vector3(1,0,0) ;
                V1 = Vector3(0,1,0) ;
            }
        }

        // Project onto shortest bbox axis,
        // and lock extrema vertices

        Vertex* vxmin = NULL ;
        float  umin = 1e5 ;
        Vertex* vxmax = NULL ;
        float  umax = -1e5 ;

        for(i=0; i<mesh_->vertices.size(); i++) {
            Vertex * V = mesh_->vertices[i] ;
            float u = V->point * V1 ;
            float v = V->point * V2 ;
            V->tex_coord = Vector2(u,v) ;
            if(u < umin) {
                vxmin = V ;
                umin = u ;
            } 
            if(u > umax) {
                vxmax = V ;
                umax = u ;
            } 
        }

        vxmin->locked = true ;
        vxmax->locked = true ;

		//This fucntion is correct
		std::cout<<"VMIN "<<vxmin->idx<<std::endl;
		std::cout<<"VMAX "<<vxmax->idx<<std::endl;
    }

    Mesh * mesh_ ;
} ;

int main(int argc, char** argv) {
    if(argc != 3) {
        std::cerr << "usage: " << argv[0] << " infile.obj outfile.obj" << std::endl ;
        return -1 ;
    }

    Mesh mesh ;
    std::cout << "Loading ..." << std::endl ;
    std::ifstream ifs (argv[1]);
	ifs >> mesh; 
	std::cout<<mesh.vertices.size()<<std::endl;
	std::cout<<mesh.faces.size()<<std::endl;
	std::cout<<mesh.edges.size()<<std::endl;

    LSCM lscm(mesh) ;
    lscm.apply() ;

    std::cout << "Saving ..." << std::endl ;
	std::ofstream ofs (argv[2]);
	ofs << mesh ;
}
